/***************************************************************************
 创建者: 华磊
 开始时间: 2020.8.14
 copyright: (C) 华友高科
 修改说明: (每次有修改就添加一条，带有 修改人，修改时间，修改描述)
 example (1) hualei 2020.5.8 类的具体实现代码编写

 ***************************************************************************/
#include "lasercalculate.h"
#include <math.h>
#include <Eigen/Geometry>

LaserCalculate::LaserCalculate()
{
    minumIntensity=0;
    maxLaserRange=100;
    minAngle=-10*M_PI;
    maxAngle=10*M_PI;
}

int LaserCalculate::laserFilterBySkip(int skipNum, LaserDataInfo &laserDataOut)
{
    if(0==skipNum)
    {
        return 0;
    }
    LaserDataInfo laserDataNew=laserDataOut;
    laserDataNew.ranges.clear();
    laserDataNew.intensities.clear();
    laserDataNew.angle_increment=laserDataNew.angle_increment*(skipNum+1);
    laserDataNew.time_increment=laserDataNew.time_increment*(skipNum+1);

    for(int i=0;i<laserDataOut.ranges.size();i+=skipNum+1)
    {
        laserDataNew.ranges.push_back(laserDataOut.ranges[i]);
        laserDataNew.intensities.push_back(laserDataOut.intensities[i]);
    }
    laserDataOut=laserDataNew;
    return 1;
}

int LaserCalculate::laserFilterByAngle(double minAngleIn, double maxAngleIn, LaserDataInfo &laserDataOut)
{
    //傲视驱动的开始角度是3.92 需要转换到【－ｐｉ,pi】
//    double startAngle=transformToPi(laserDataOut.angle_min);傲视雷达需要改驱动。
    double startAngle=laserDataOut.angle_min;
    double maxRange=laserDataOut.range_max-0.5;
    for(int i=0;i<laserDataOut.ranges.size();i++)
    {
        double tmpAngle=startAngle+i*laserDataOut.angle_increment;

        if(tmpAngle<minAngleIn || tmpAngle> maxAngleIn || laserDataOut.ranges[i]>maxRange)//为lms1xx增加距离处理。
        {
            laserDataOut.ranges[i]=laserDataOut.range_max+10000;
        }
    }

    //test
//    int skip=200;
//    laserDataOut.angle_min+=laserDataOut.angle_increment*skip;
//    laserDataOut.angle_max-=laserDataOut.angle_increment*skip;
//    for(int i=0;i<skip*2;i++)
//    {
//        laserDataOut.ranges.pop_back();
//        laserDataOut.intensities.pop_back();
//    }


    return 1;
}

int LaserCalculate::tranlatePointCloutToVehicleOrigin(LaserScanDeviceInfo deviceInfo, const PointCloudData_3D &pointCloudIn,
                                                      PointCloudData_3D &pointCloudOut)
{

    Eigen::AngleAxisd rollAngle(deviceInfo.pose_roll, Eigen::Vector3d::UnitX());
    Eigen::AngleAxisd pitchAngle(deviceInfo.pose_pitch, Eigen::Vector3d::UnitY());
    Eigen::AngleAxisd yawAngle(deviceInfo.pose_yaw, Eigen::Vector3d::UnitZ());

    Eigen::Quaterniond q = yawAngle*pitchAngle*rollAngle;
    Eigen::Matrix3d rotateMa = q.matrix();

    Eigen::Isometry3d frame_laser_in_baselink;
    frame_laser_in_baselink= rotateMa;
    frame_laser_in_baselink.translation()=Eigen::Vector3d(deviceInfo.pose_x, deviceInfo.pose_y, deviceInfo.pose_z);
//    Eigen::Isometry3d laserPointInLaserlink;
//    Eigen::Isometry3d laserPointInBaselink;
//    Eigen::Vector3d laserPoint;
    Eigen::Vector3d p1;
    Eigen::Vector3d p2;

    int pointCount=pointCloudIn.point_x.size();
    pointCloudOut.point_x.resize(pointCount);
    pointCloudOut.point_y.resize(pointCount);
    pointCloudOut.point_z.resize(pointCount);

    for(int i=0;i<pointCount;i++)
    {
        p1=Eigen::Vector3d(pointCloudIn.point_x[i],pointCloudIn.point_y[i], pointCloudIn.point_z[i]);
//        laserPointInLaserlink.translation() = Eigen::Vector3d(pointCloudIn.point_x[i],pointCloudIn.point_y[i], pointCloudIn.point_z[i]);
//        laserPointInBaselink=frame_laser_in_baselink*laserPointInLaserlink;//?应该是对的
        p2=frame_laser_in_baselink*p1;
        pointCloudOut.point_x[i]=p2[0];
        pointCloudOut.point_y[i]=p2[1];
        pointCloudOut.point_z[i]=p2[2];
//        laserPoint=laserPointInBaselink.translation();
//        pointCloudOut.point_x[i]=laserPoint[0];
//        pointCloudOut.point_y[i]=laserPoint[1];
//        pointCloudOut.point_z[i]=laserPoint[2];
    }



    return 1;


}


//int LaserCalculate::tranlatePointCloutToVehicleOrigin(LaserScanDeviceInfo deviceInfo, const PointCloudData_3D &pointCloudIn,
//                                                      PointCloudData_3D &pointCloudOut)
//{

//    Eigen::AngleAxisd rollAngle(deviceInfo.pose_roll, Eigen::Vector3d::UnitX());
//    Eigen::AngleAxisd pitchAngle(deviceInfo.pose_pitch, Eigen::Vector3d::UnitY());
//    Eigen::AngleAxisd yawAngle(deviceInfo.pose_yaw, Eigen::Vector3d::UnitZ());

//    Eigen::Quaterniond q = yawAngle*pitchAngle*rollAngle;
//    Eigen::Matrix3d rotateMa = q.matrix();

//    Eigen::Isometry3d frame_laser_in_baselink;
//    frame_laser_in_baselink= rotateMa;
//    frame_laser_in_baselink.translation()=Eigen::Vector3d(deviceInfo.pose_x, deviceInfo.pose_y, deviceInfo.pose_z);
//    Eigen::Isometry3d laserPointInLaserlink;
//    Eigen::Isometry3d laserPointInBaselink;
//    Eigen::Vector3d laserPoint;
////    Eigen::Vector3d p1;
////    Eigen::Vector3d p2;

//    int pointCount=pointCloudIn.point_x.size();
//    pointCloudOut.point_x.resize(pointCount);
//    pointCloudOut.point_y.resize(pointCount);
//    pointCloudOut.point_z.resize(pointCount);

//    for(int i=0;i<pointCount;i++)
//    {
//        laserPointInLaserlink.translation() = Eigen::Vector3d(pointCloudIn.point_x[i],pointCloudIn.point_y[i], pointCloudIn.point_z[i]);
//        laserPointInBaselink=frame_laser_in_baselink*laserPointInLaserlink;//?应该是对的
//        laserPoint=laserPointInBaselink.translation();
//        pointCloudOut.point_x[i]=laserPoint[0];
//        pointCloudOut.point_y[i]=laserPoint[1];
//        pointCloudOut.point_z[i]=laserPoint[2];
//    }



//    return 1;


//}


//int LaserCalculate::tranlatePointCloutToVehicleOrigin(LaserScanDeviceInfo deviceInfo, const PointCloudData_3D &pointCloudIn,
//                                                      PointCloudData_3D &pointCloudOut)
//{



//        Eigen::AngleAxisd rollAngle(deviceInfo.pose_roll, Eigen::Vector3d::UnitX());
//        Eigen::AngleAxisd pitchAngle(deviceInfo.pose_pitch, Eigen::Vector3d::UnitY());
//        Eigen::AngleAxisd yawAngle(deviceInfo.pose_yaw, Eigen::Vector3d::UnitZ());

//        Eigen::Quaterniond q = yawAngle*pitchAngle*rollAngle;
//        Eigen::Matrix3d rotateMa = q.matrix();

//        Eigen::Matrix4f tmpMatrix=Eigen::Matrix4f::Identity();

//        tmpMatrix(0,0)=rotateMa(0,0);
//        tmpMatrix(0,1)=rotateMa(0,1);
//        tmpMatrix(0,2)=rotateMa(0,2);
//        tmpMatrix(1,0)=rotateMa(1,0);
//        tmpMatrix(1,1)=rotateMa(1,1);
//        tmpMatrix(1,2)=rotateMa(1,2);
//        tmpMatrix(2,0)=rotateMa(2,0);
//        tmpMatrix(2,1)=rotateMa(2,1);
//        tmpMatrix(2,2)=rotateMa(2,2);

//        tmpMatrix(0,3)=deviceInfo.pose_x;
//        tmpMatrix(1,3)=deviceInfo.pose_y;
//        tmpMatrix(2,3)=deviceInfo.pose_z;

//    Eigen::Matrix4f  p1;
//    Eigen::Matrix4f p2;
//    p1(0,0)=1;
//    p1(0,1)=0;
//    p1(0,2)=0;
//    p1(1,0)=1;
//    p1(1,1)=1;
//    p1(1,2)=0;
//    p1(2,0)=0;
//    p1(2,1)=0;
//    p1(2,2)=1;

//    int pointCount=pointCloudIn.point_x.size();
//    pointCloudOut.point_x.resize(pointCount);
//    pointCloudOut.point_y.resize(pointCount);
//    pointCloudOut.point_z.resize(pointCount);

//    for(int i=0;i<pointCount;i++)
//    {
//        p1(0,3)=pointCloudIn.point_x[i];
//        p1(1,3)=pointCloudIn.point_y[i];
//        p1(2,3)=pointCloudIn.point_z[i];
//        p2=tmpMatrix*p1;//?应该是对的

//        pointCloudOut.point_x[i]=p2(0,3);
//        pointCloudOut.point_y[i]=p2(1,3);
//        pointCloudOut.point_z[i]=p2(2,3);

//    }



//    return 1;


//}


//int LaserCalculate::tranlatePointCloutToVehicleOrigin(LaserScanDeviceInfo deviceInfo, const PointCloudData_3D &pointCloudIn,
//                                                      PointCloudData_3D &pointCloudOut)
//{


//    Eigen::AngleAxisd rollAngle(deviceInfo.pose_roll, Eigen::Vector3d::UnitX());
//    Eigen::AngleAxisd pitchAngle(deviceInfo.pose_pitch, Eigen::Vector3d::UnitY());
//    Eigen::AngleAxisd yawAngle(deviceInfo.pose_yaw, Eigen::Vector3d::UnitZ());

//    Eigen::Quaterniond q = yawAngle*pitchAngle*rollAngle;
//    Eigen::Matrix3d rotateMa = q.matrix();

//    Eigen::Matrix4f tmpMatrix=Eigen::Matrix4f::Identity();


//    tmpMatrix(0,0)=rotateMa(0,0);
//    tmpMatrix(0,1)=rotateMa(0,1);
//    tmpMatrix(0,2)=rotateMa(0,2);
//    tmpMatrix(1,0)=rotateMa(1,0);
//    tmpMatrix(1,1)=rotateMa(1,1);
//    tmpMatrix(1,2)=rotateMa(1,2);
//    tmpMatrix(2,0)=rotateMa(2,0);
//    tmpMatrix(2,1)=rotateMa(2,1);
//    tmpMatrix(2,2)=rotateMa(2,2);

//    tmpMatrix(0,3)=deviceInfo.pose_x;
//    tmpMatrix(1,3)=deviceInfo.pose_y;
//    tmpMatrix(2,3)=deviceInfo.pose_z;

//        Eigen::Vector4d p1;
//        Eigen::Vector4d p2;
//        int pointCount=pointCloudIn.point_x.size();
//        pointCloudOut.point_x.resize(pointCount);
//        pointCloudOut.point_y.resize(pointCount);
//        pointCloudOut.point_z.resize(pointCount);

//        for(int i=0;i<pointCount;i++)
//        {
////           p1 = Eigen::Vector3d(pointCloudIn.point_x[i],pointCloudIn.point_y[i], pointCloudIn.point_z[i]);
//           p1 = Eigen::Vector4d(pointCloudIn.point_x[i],pointCloudIn.point_y[i], pointCloudIn.point_z[i],1);

//            p2=tmpMatrix*p1;//?

//            pointCloudOut.point_x[i]=p2[0];
//            pointCloudOut.point_y[i]=p2[1];
//            pointCloudOut.point_z[i]=p2[2];


//        }


//    return 1;


//}

int LaserCalculate::setLaserIntensityFilter(double minumIntensityIn, double maxLaserRangeIn, double minAngleIn, double maxAngleIn)
{
    minumIntensity=minumIntensityIn;
    maxLaserRange=maxLaserRangeIn;
    minAngle=minAngleIn;
    maxAngle=maxAngleIn;
    return 1;
}

int LaserCalculate::laserFilterByIntensity(const LaserDataInfo &frontLaserDataIn,
                                           LaserDataInfo &frontLaserDataOut)
{

    return laserFilterByIntensity(minumIntensity,maxLaserRange,minAngle,maxAngle,frontLaserDataIn,frontLaserDataOut);
}

int LaserCalculate::laserFilterByIntensity(double minumIntensityIn, double maxLaserRangeIn, double minAngleIn, double maxAngleIn,
                                           const LaserDataInfo &frontLaserDataIn, LaserDataInfo &frontLaserDataOut)
{
    frontLaserDataOut=frontLaserDataIn;
    for(int i=0;i<frontLaserDataOut.intensities.size();i++)
    {
        double tmpAngle=frontLaserDataOut.angle_min+i*frontLaserDataOut.angle_increment;
        if(frontLaserDataOut.intensities[i]<minumIntensityIn || frontLaserDataOut.ranges[i]>maxLaserRangeIn
                || tmpAngle<minAngleIn || tmpAngle> maxAngleIn)
        {
            frontLaserDataOut.ranges[i]=frontLaserDataOut.range_max+10000;
        }
    }

    return 1;
}

int LaserCalculate::findReflectiveStripe(double minumIntensity, double minStripeWidth, double maxStripeWidth,
                                         const LaserDataInfo &frontLaserDataIn, std::vector<double> &posX,
                                         std::vector<double> &posY, std::vector<double> &posRotate)
{

}

double LaserCalculate::transformToPi(double angleIn)
{
    //    int turns=angleIn/M_PI/2;//可能bug 参考getTurnsFromRad
        int turns;

        if(0<angleIn)
        {
            turns=(angleIn+M_PI) /(2*M_PI);
        }
        else
        {
            turns=(angleIn-M_PI) /(2*M_PI);
        }

        //    angleIn=angleIn-turns*M_PI;//错误
            angleIn=angleIn-turns*M_PI*2;

        if(angleIn>=-M_PI && angleIn<=M_PI)
        {
            return angleIn;
        }
        else if(angleIn<-M_PI)
        {
            return angleIn+2*M_PI;
        }
        else if(angleIn>M_PI)
        {
            return angleIn-2*M_PI;
        }

        return angleIn;
}

int LaserCalculate::transformToCartesianPosition(int skipInterval,const LaserDataInfo &laserData, std::vector<double> &posX,
                                                std::vector<double> &posY)
{
    //以激光器的绝对零角度为ｘ轴。

    posX.clear();
    posY.clear();

    double tmpX,tmpY;

    //除去不在有效范围内的无效数据
    for(int i=0;i<laserData.ranges.size();i=i+1+skipInterval)
    {
        if(laserData.ranges[i]<laserData.range_max && laserData.ranges[i]>laserData.range_min)
        {
            //x y轴可能是反的？
            tmpX=laserData.ranges[i]*cos(laserData.angle_min+laserData.angle_increment*i);
            tmpY=laserData.ranges[i]*sin(laserData.angle_min+laserData.angle_increment*i);
            posX.push_back(tmpX);//求baselink在ｗｏｒld使用
            posY.push_back(tmpY);
//            posX.push_back(tmpY);//求ｖmark在ｗｏｒld使用
//            posY.push_back(tmpX);
            //考虑车体速度移动导致的数据变形，　laserData.time_increment 。//need todo hualei
        }

    }
    return 1;



}

int LaserCalculate::deleteFlyPoint(double allowMiniumAngle, const LaserDataInfo &laserDataIn, LaserDataInfo &laserDataOut)
{
    LaserDataInfo tmp_laserDataIn=laserDataIn;

    double deltaD;
    double kp=sin(laserDataIn.angle_increment);
    double deltaL;
    double deltaA;
    double deltaA_old=M_PI_2;

    laserDataOut=laserDataIn;

    for(int i=0;i<tmp_laserDataIn.ranges.size()-1;i++)
    {
        //有效范围内
        if(tmp_laserDataIn.ranges[i]<tmp_laserDataIn.range_max && tmp_laserDataIn.ranges[i]>tmp_laserDataIn.range_min)
        {
            deltaD=tmp_laserDataIn.ranges[i]*kp;
            deltaL=fabs(tmp_laserDataIn.ranges[i]-tmp_laserDataIn.ranges[i+1]);
            deltaA=atan2(deltaD,deltaL);
            if(allowMiniumAngle>deltaA && allowMiniumAngle>deltaA_old)
            {
//                if(i-3>=0)
//                {
//                    laserDataOut.ranges[i-3]=tmp_laserDataIn.range_max+10000;
//                }
//                if(i-2>=0)
//                {
//                    laserDataOut.ranges[i-2]=tmp_laserDataIn.range_max+10000;
//                }
                if(i-1>=0)
                {
                    laserDataOut.ranges[i-1]=tmp_laserDataIn.range_max+10000;
                }
                laserDataOut.ranges[i]=tmp_laserDataIn.range_max+10000;
                laserDataOut.ranges[i+1]=tmp_laserDataIn.range_max+10000;
//                if(i+2<tmp_laserDataIn.ranges.size())
//                {
//                    laserDataOut.ranges[i+2]=tmp_laserDataIn.range_max+10000;
//                }
//                if(i+3<tmp_laserDataIn.ranges.size())
//                {
//                    laserDataOut.ranges[i+3]=tmp_laserDataIn.range_max+10000;
//                }
            }
            deltaA_old=deltaA;
        }

    }
    return 1;
}
